{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Script to get the jacobian matrix which under the .ipynb format"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "import sympy as sym\n",
    "import numpy as np"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y, z, w, h, l, v, yaw, ry_rate = sym.symbols(\"x y z w h l v yaw ry_rate\")\n",
    "next_x, next_y, next_z, next_w, next_h, next_l, next_v, next_yaw, next_ry_rate = sym.symbols(\"next_x next_y next_z next_w next_h next_l next_v next_yaw next_ry_rate\")\n",
    "dt = sym.symbols(\"dt\")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [],
   "source": [
    "next_z, next_w, next_h, next_l, next_v, next_ry_rate = z, w, h, l, v, ry_rate\n",
    "next_yaw = yaw + ry_rate * dt"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 4,
   "metadata": {},
   "outputs": [],
   "source": [
    "vt, yawt, ita, t = sym.symbols(\"vt yawt ita t\")\n",
    "\"\"\"\n",
    "yawt = yaw + ry_rate * (ita - (t - 1) * dt)\n",
    "next_x = x + sym.integrate(v * sym.cos(yawt), (ita, (t - 1) * dt, t * dt))\n",
    "next_y = y + sym.integrate(v * sym.sin(yawt), (ita, (t - 1) * dt, t * dt))\n",
    "\"\"\"\n",
    "# warp raw transition time from [(t-1)*dt, t*dt] to [0, dt], for quick solution\n",
    "vt = v\n",
    "yawt = yaw + ry_rate * (ita)\n",
    "next_x = x + sym.integrate(vt * sym.cos(yawt), (ita, 0, dt))\n",
    "next_y = y + sym.integrate(vt * sym.sin(yawt), (ita, 0, dt))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Analytical solutions for displacements can also be obtained"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle x + \\begin{cases} - \\frac{v \\sin{\\left(yaw \\right)}}{ry_{rate}} + \\frac{v \\sin{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}} & \\text{for}\\: ry_{rate} > -\\infty \\wedge ry_{rate} < \\infty \\wedge ry_{rate} \\neq 0 \\\\dt v \\cos{\\left(yaw \\right)} & \\text{otherwise} \\end{cases}$"
      ],
      "text/plain": [
       "x + Piecewise((-v*sin(yaw)/ry_rate + v*sin(dt*ry_rate + yaw)/ry_rate, (ry_rate > -oo) & (ry_rate < oo) & Ne(ry_rate, 0)), (dt*v*cos(yaw), True))"
      ]
     },
     "execution_count": 5,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "next_x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 6,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle y + \\begin{cases} \\frac{v \\cos{\\left(yaw \\right)}}{ry_{rate}} - \\frac{v \\cos{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}} & \\text{for}\\: ry_{rate} > -\\infty \\wedge ry_{rate} < \\infty \\wedge ry_{rate} \\neq 0 \\\\dt v \\sin{\\left(yaw \\right)} & \\text{otherwise} \\end{cases}$"
      ],
      "text/plain": [
       "y + Piecewise((v*cos(yaw)/ry_rate - v*cos(dt*ry_rate + yaw)/ry_rate, (ry_rate > -oo) & (ry_rate < oo) & Ne(ry_rate, 0)), (dt*v*sin(yaw), True))"
      ]
     },
     "execution_count": 6,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "next_y"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 7,
   "metadata": {},
   "outputs": [],
   "source": [
    "funcs = sym.Matrix([next_x, next_y, next_z, next_w, next_l, next_h, next_v, next_yaw, next_ry_rate])\n",
    "args = sym.Matrix([x, y, z, w, l, h, v, yaw, ry_rate])\n",
    "res = funcs.jacobian(args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & \\begin{cases} - \\frac{\\sin{\\left(yaw \\right)}}{ry_{rate}} + \\frac{\\sin{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}} & \\text{for}\\: ry_{rate} > -\\infty \\wedge ry_{rate} < \\infty \\wedge ry_{rate} \\neq 0 \\\\dt \\cos{\\left(yaw \\right)} & \\text{otherwise} \\end{cases} & \\begin{cases} - \\frac{v \\cos{\\left(yaw \\right)}}{ry_{rate}} + \\frac{v \\cos{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}} & \\text{for}\\: ry_{rate} > -\\infty \\wedge ry_{rate} < \\infty \\wedge ry_{rate} \\neq 0 \\\\- dt v \\sin{\\left(yaw \\right)} & \\text{otherwise} \\end{cases} & \\begin{cases} \\frac{dt v \\cos{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}} + \\frac{v \\sin{\\left(yaw \\right)}}{ry_{rate}^{2}} - \\frac{v \\sin{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}^{2}} & \\text{for}\\: ry_{rate} > -\\infty \\wedge ry_{rate} < \\infty \\wedge ry_{rate} \\neq 0 \\\\0 & \\text{otherwise} \\end{cases}\\\\0 & 1 & 0 & 0 & 0 & 0 & \\begin{cases} \\frac{\\cos{\\left(yaw \\right)}}{ry_{rate}} - \\frac{\\cos{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}} & \\text{for}\\: ry_{rate} > -\\infty \\wedge ry_{rate} < \\infty \\wedge ry_{rate} \\neq 0 \\\\dt \\sin{\\left(yaw \\right)} & \\text{otherwise} \\end{cases} & \\begin{cases} - \\frac{v \\sin{\\left(yaw \\right)}}{ry_{rate}} + \\frac{v \\sin{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}} & \\text{for}\\: ry_{rate} > -\\infty \\wedge ry_{rate} < \\infty \\wedge ry_{rate} \\neq 0 \\\\dt v \\cos{\\left(yaw \\right)} & \\text{otherwise} \\end{cases} & \\begin{cases} \\frac{dt v \\sin{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}} - \\frac{v \\cos{\\left(yaw \\right)}}{ry_{rate}^{2}} + \\frac{v \\cos{\\left(dt ry_{rate} + yaw \\right)}}{ry_{rate}^{2}} & \\text{for}\\: ry_{rate} > -\\infty \\wedge ry_{rate} < \\infty \\wedge ry_{rate} \\neq 0 \\\\0 & \\text{otherwise} \\end{cases}\\\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 1 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & dt\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 1\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, 0, 0, 0, 0, Piecewise((-sin(yaw)/ry_rate + sin(dt*ry_rate + yaw)/ry_rate, (ry_rate > -oo) & (ry_rate < oo) & Ne(ry_rate, 0)), (dt*cos(yaw), True)), Piecewise((-v*cos(yaw)/ry_rate + v*cos(dt*ry_rate + yaw)/ry_rate, (ry_rate > -oo) & (ry_rate < oo) & Ne(ry_rate, 0)), (-dt*v*sin(yaw), True)), Piecewise((dt*v*cos(dt*ry_rate + yaw)/ry_rate + v*sin(yaw)/ry_rate**2 - v*sin(dt*ry_rate + yaw)/ry_rate**2, (ry_rate > -oo) & (ry_rate < oo) & Ne(ry_rate, 0)), (0, True))],\n",
       "[0, 1, 0, 0, 0, 0,  Piecewise((cos(yaw)/ry_rate - cos(dt*ry_rate + yaw)/ry_rate, (ry_rate > -oo) & (ry_rate < oo) & Ne(ry_rate, 0)), (dt*sin(yaw), True)),  Piecewise((-v*sin(yaw)/ry_rate + v*sin(dt*ry_rate + yaw)/ry_rate, (ry_rate > -oo) & (ry_rate < oo) & Ne(ry_rate, 0)), (dt*v*cos(yaw), True)), Piecewise((dt*v*sin(dt*ry_rate + yaw)/ry_rate - v*cos(yaw)/ry_rate**2 + v*cos(dt*ry_rate + yaw)/ry_rate**2, (ry_rate > -oo) & (ry_rate < oo) & Ne(ry_rate, 0)), (0, True))],\n",
       "[0, 0, 1, 0, 0, 0,                                                                                                                                      0,                                                                                                                                             0,                                                                                                                                                                          0],\n",
       "[0, 0, 0, 1, 0, 0,                                                                                                                                      0,                                                                                                                                             0,                                                                                                                                                                          0],\n",
       "[0, 0, 0, 0, 1, 0,                                                                                                                                      0,                                                                                                                                             0,                                                                                                                                                                          0],\n",
       "[0, 0, 0, 0, 0, 1,                                                                                                                                      0,                                                                                                                                             0,                                                                                                                                                                          0],\n",
       "[0, 0, 0, 0, 0, 0,                                                                                                                                      1,                                                                                                                                             0,                                                                                                                                                                          0],\n",
       "[0, 0, 0, 0, 0, 0,                                                                                                                                      0,                                                                                                                                             1,                                                                                                                                                                         dt],\n",
       "[0, 0, 0, 0, 0, 0,                                                                                                                                      0,                                                                                                                                             0,                                                                                                                                                                          1]])"
      ]
     },
     "execution_count": 8,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "res"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "metadata": {},
   "outputs": [],
   "source": [
    "mea_funcs = sym.Matrix([x, y, z, w, l, h, v * sym.cos(yaw), v * sym.sin(yaw), yaw])\n",
    "mea_res = mea_funcs.jacobian(args)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 10,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/latex": [
       "$\\displaystyle \\left[\\begin{matrix}1 & 0 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 1 & 0 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 1 & 0 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 1 & 0 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 1 & 0 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 1 & 0 & 0 & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & \\cos{\\left(yaw \\right)} & - v \\sin{\\left(yaw \\right)} & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & \\sin{\\left(yaw \\right)} & v \\cos{\\left(yaw \\right)} & 0\\\\0 & 0 & 0 & 0 & 0 & 0 & 0 & 1 & 0\\end{matrix}\\right]$"
      ],
      "text/plain": [
       "Matrix([\n",
       "[1, 0, 0, 0, 0, 0,        0,           0, 0],\n",
       "[0, 1, 0, 0, 0, 0,        0,           0, 0],\n",
       "[0, 0, 1, 0, 0, 0,        0,           0, 0],\n",
       "[0, 0, 0, 1, 0, 0,        0,           0, 0],\n",
       "[0, 0, 0, 0, 1, 0,        0,           0, 0],\n",
       "[0, 0, 0, 0, 0, 1,        0,           0, 0],\n",
       "[0, 0, 0, 0, 0, 0, cos(yaw), -v*sin(yaw), 0],\n",
       "[0, 0, 0, 0, 0, 0, sin(yaw),  v*cos(yaw), 0],\n",
       "[0, 0, 0, 0, 0, 0,        0,           1, 0]])"
      ]
     },
     "execution_count": 10,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "mea_res"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.13"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
